#include "qmc5883l.h"               // 包含qmc5883l的头文件
#include "driver/i2c.h"             // 包含I2C驱动的头文件
#include "esp_log.h"                // 包含esp日志输出的头文件
#include "myi2c.h"                  // 包含自定义的I2C驱动的头文件
#include "freertos/FreeRTOS.h"      // 包含FreeRTOS的头文件
#include "freertos/task.h"          // 包含FreeRTOS任务相关的头文件
#include <math.h>                   // 包含数学库的头文件

static const char *标签 = "QMC5883L";    // 定义日志标签
// 定义QMC5883L的寄存器地址
esp_err_t qmc5883L_register_read(uint8_t 寄存器地址, uint8_t *数据, size_t 长度)
{
    return i2c_master_write_read_device(I2C主端口, QMC5883L传感器地址,  &寄存器地址, 1, 数据, 长度, I2C自动超时 / portTICK_PERIOD_MS);
}
// 写一个字节到QMC5883L的寄存器
esp_err_t qmc5883L_register_write_byte(uint8_t 寄存器地址, uint8_t 数据)
{
    uint8_t write_buf[2] = {寄存器地址, 数据};

    return i2c_master_write_to_device(I2C主端口, QMC5883L传感器地址, write_buf, sizeof(write_buf), I2C自动超时 / portTICK_PERIOD_MS);
}

// 初始化QMC5883L
void qmc5883l_init(void)
{
    uint8_t id = 0;

    while (id != 0xff)  // 确定ID号是否正确
    {
        qmc5883L_register_read(QMC5883L芯片ID, &id ,1);
        vTaskDelay(1000 / portTICK_PERIOD_MS);
    }
    ESP_LOGI(标签, "QMC5883L OK!");
    qmc5883L_register_write_byte(QMC5883L控制2, 0x80); // 复位芯片 
    vTaskDelay(10 / portTICK_PERIOD_MS);
    qmc5883L_register_write_byte(QMC5883L控制1, 0x05); //Continuous模式 50Hz 
    qmc5883L_register_write_byte(QMC5883L控制2, 0x00); 
    qmc5883L_register_write_byte(QMC5883L采样率, 0x01);   // 100Hz 采样率
}

// 读取QMC5883L的XYZ轴数据
void qmc5883l_read_xyz(t_sQMC5883L *p)
{
    uint8_t 状态, 数据就绪=0;
    int16_t mag_reg[3];

    qmc5883L_register_read(QMC5883L_状态, &状态, 1); // 读状态寄存器 

    if (状态 & 0x01)
    {
        数据就绪 = 1;
    }
    if (数据就绪 == 1)
    {
        数据就绪 = 0;
        qmc5883L_register_read(QMC5883L_X坐标低, (uint8_t *)mag_reg, 6);

        p->x轴 = mag_reg[0];      // 读取X轴数据
        p->y轴 = mag_reg[1];      // 读取Y轴数据
        p->z轴 = mag_reg[2];      // 读取Z轴数据
        //p->方位角 = (float)atan2(p->y轴, p->x轴) * 180.0 / 3.1415926 + 180.0;
    }
}

// 计算QMC5883L的方位角
void qmc5883l_fetch_azimuth(t_sQMC5883L *p)
{
    qmc5883l_read_xyz(p);

    p->方位角 = (float)atan2(p->y轴, p->x轴) * 180.0 / 3.1415926 + 180.0;
}